#include "DM/J4310.h"
#include "../utils.hpp"
#include <array>
#include <algorithm>
#include <format>
#include <cstring>

namespace MM {
    constexpr float DM_V_MIN = -45.0f;
    constexpr float DM_V_MAX = 45.0f;
    constexpr float DM_P_MIN = -12.5f;
    constexpr float DM_P_MAX = 12.5f;
    constexpr float DM_T_MIN = -18.0f;
    constexpr float DM_T_MAX = 18.0f;
    constexpr float DM_KP_MIN = 0.0f;
    constexpr float DM_KP_MAX = 500.0f;
    constexpr float DM_KD_MIN = 0.0f;
    constexpr float DM_KD_MAX = 5.0f;

    constexpr float uint_to_float(const int x_int, const float x_min, const float x_max, const int bits) {
        /// converts unsigned int to float, given range and number of bits ///
        const float span = x_max - x_min;
        const float offset = x_min;
        return static_cast<float>(x_int) * span / static_cast<float>((1 << bits) - 1) + offset;
    }

    constexpr int float_to_uint(const float x, const float x_min, const float x_max, const int bits) {
        /// Converts a float to an unsigned int, given range and number of bit
        const float span = x_max - x_min;
        const float offset = x_min;
        return static_cast<int>((x - offset) * static_cast<float>((1 << bits) - 1) / span);
    }

    J4310Status::J4310Status(const can_frame &frame) {
        const auto *data = reinterpret_cast<const uint8_t *>(frame.data);
        auto tmp = static_cast<uint16_t>((data[1] << 8) | data[2]);
        position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
        tmp = static_cast<uint16_t>((data[3] << 4) | data[4] >> 4);
        velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
        tmp = static_cast<uint16_t>(((data[4] & 0x0f) << 8) | data[5]);
        torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);

        ID = data[0] & 0x0F;
        status = static_cast<DMStatus>((frame.data[0] >> 4) & 0x0F);
        temperature_MOS = data[6];
        temperature_Rotor = data[7];
    }

    using std::format, std::make_format_args, std::format_args;

    string J4310Status::format() {
        return vformat("ID: {:X}, Position: {}, Velocity: {}, Torque: {}, temperature: {},{} deg, ",
                       // "Status: {}",
                       format_args(make_format_args(ID, position, velocity, torque, temperature_MOS,
                                                    temperature_Rotor)));
        // status)));
        //TODO: magic enum
    }

    J4310MITControl::operator can_frame() {
        can_frame frame{};
        frame.can_id = frameId.value_or(0x01);
        const uint16_t pos_temp = float_to_uint(position, DM_P_MAX, DM_P_MIN, 16);
        const uint16_t vel_temp = float_to_uint(velocity, DM_V_MAX, DM_V_MIN, 12);
        const uint16_t tor_temp = float_to_uint(torque, DM_T_MAX, DM_T_MIN, 12);
        const uint16_t kp_temp = float_to_uint(Kp, DM_KP_MAX, DM_KP_MIN, 12);
        const uint16_t kd_temp = float_to_uint(Kd, DM_KD_MAX, DM_KD_MIN, 12);

        frame.len = 8;
        frame.data[0] = pos_temp >> 8;
        frame.data[1] = pos_temp;
        frame.data[2] = vel_temp >> 4;
        frame.data[3] = ((vel_temp & 0xF) << 4) | (kp_temp >> 8);
        frame.data[4] = kp_temp;
        frame.data[5] = (kd_temp >> 4);
        frame.data[6] = ((kd_temp & 0xF) << 4) | (tor_temp >> 8);
        frame.data[7] = tor_temp;
        return frame;
    }

    string J4310MITControl::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X}, Position: {}, Velocity: {}, Torque: {}, Kp: {}, Kd: {}",
                       make_format_args(canId, position, velocity, torque, Kp,
                                        Kd));
    }

    J4310PosVelControl::operator can_frame() {
        can_frame frame{};

        frame.can_id = frameId.value_or(0x01) + 0x100;
        frame.len = 8;
        std::memcpy(frame.data, &position, sizeof(float));
        std::memcpy(frame.data + 4, &velocity, sizeof(float));
        return frame;
    }

    string J4310PosVelControl::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X}, Position: {}, Velocity: {}",
                       make_format_args(canId, position, velocity));
    }

    J4310VelControl::operator can_frame() {
        can_frame frame{};

        frame.can_id = frameId.value_or(0x01) + 0x200;
        frame.len = 4;
        std::memcpy(frame.data, &velocity, sizeof(float));
        return frame;
    }

    string J4310VelControl::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X}, Velocity: {}",
                       make_format_args(canId, velocity));
    }

    J4310Enable::operator can_frame() {
        constexpr std::array<uint8_t, 8> data_{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
        can_frame frame{};
        frame.can_id = frameId.value_or(0x01) + 0x100;
        frame.len = 8;
        std::ranges::copy(data_, frame.data);
        return frame;
    }

    string J4310Enable::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X} is being Enabled.", make_format_args(canId));
    }

    J4310Disable::operator can_frame() {
        constexpr std::array<uint8_t, 8> data_{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
        can_frame frame{};
        frame.can_id = frameId.value_or(0x01) + 0x100;
        frame.len = 8;
        std::ranges::copy(data_, frame.data);
        return frame;
    }

    string J4310Disable::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X} is being Disabled.", make_format_args(canId));
    }

    J4310ZeroPosition::operator can_frame() {
        constexpr std::array<uint8_t, 8> data_{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE};
        can_frame frame{};
        frame.can_id = frameId.value_or(0x01) + 0x100;
        frame.len = 8;
        std::ranges::copy(data_, frame.data);
        return frame;
    }

    string J4310ZeroPosition::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X} 's Position is being reset to 0.", make_format_args(canId));
    }

    J4310CleanError::operator can_frame() {
        constexpr std::array<uint8_t, 8> data_{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB};
        can_frame frame{};
        frame.can_id = frameId.value_or(0x01) + 0x100;
        frame.len = 8;
        std::ranges::copy(data_, frame.data);
        return frame;
    }

    string J4310CleanError::format() {
        uint16_t canId = frameId.value_or(0x01);
        return vformat("CanID: 0x{:X} 's Error is being Cleaned.", make_format_args(canId));
    }

    J4310::~J4310() {
        logger.force_flush();
    }

    using std::to_string, std::make_unique, std::move;

    J4310::J4310(string canInterface, const uint16_t canId,
                 const uint16_t masterId, optional<string> &&loggerId, const bool loggingToTextfile,
                 const bool loggingToConsole) : interface(std::move(canInterface)),
                                                id(loggerId.value_or(
                                                    to_string(random_number(0, 2000)))),
                                                canId(canId), masterId(masterId),
                                                logger(
                                                    [&] {
                                                        const string filename =
                                                                "J4310_" + loggerId.
                                                                value_or(
                                                                    to_string(
                                                                        random_number(
                                                                            0, 2000)));
                                                        const string filepath =
                                                                string(MM_LOG_DIR) +
                                                                "/J4310/" + filename;
                                                        return log::create_log(filepath,
                                                                               configGenerate(
                                                                                   std::move(filepath),
                                                                                   loggingToConsole,
                                                                                   loggingToTextfile)
                                                        );
                                                    }()) {
        can_driver = make_unique<canpp::CanDriver<
        > >((canpp::CanDriverConfig{
            interface, id, std::set{masterId}
        }));
        can_driver->registerReadingCallback([&](can_frame &&frame) {
            auto msg = static_cast<J4310Status>(frame);
            fuse_check(msg);
            (void) logger.info("Receaving Status:\n {}", msg.format());
            statuses.insert(move(msg));
        });

        (void) logger.info("J4310 successfully inited!");
    }

    void J4310::control(J4310ControlFrame &&frame) const noexcept {
        if (frame.frameId == nullopt) {
            frame.frameId = canId;
        }
        (void) logger.info("Sending control frame:\n {}", frame.format());
        can_driver->canSendMessage(move(static_cast<can_frame>(frame)));
    }

    void J4310::fuse_check(const J4310Status &status) {
        if (status.temperature_MOS >= MM_FUSE_TEMPERATURE || status.temperature_Rotor >= MM_FUSE_TEMPERATURE) {
            throw std::runtime_error(vformat(
                "J4310 ID: 0x{:X} has exceeded fuse temperature at: {},{} degree, please check the hardware",
                make_format_args(status.ID, status.temperature_MOS, status.temperature_Rotor)));
        }
    }
}
